#include <p32xxxx.h>
#include <plib.h>


// DEVCFG1
#pragma config FNOSC = FRCPLL           // Internal Fast RC Oscillator with PLL module via Postscaler
#pragma config FPLLIDIV = DIV_2         //PLL Input Divider
#pragma config FPLLMUL = MUL_20         // PLL Multiplier (20x Multiplier)
#pragma config FPLLODIV = DIV_2         // System PLL Output Clock Divider (PLL Divide by 2)
#pragma config FPBDIV = DIV_1           //Peripheral Bus Clock Divider
#pragma config FSOSCEN = OFF            // Secondary Oscillator Off
#define SYS_FREQ (40000000L)            // 40MHz

main() {
    TRISB = 0b1111111111110000;
    PPSUnLock;
    PPSOutput(1, RPA0, OC1); //OUTPUT COMPARE1 Pin RPA0
    PPSOutput(2, RPA1, OC2); //OUTPUT COMPARE2 Pin RPA1
    PPSInput(2, IC3, RPB11); //INPUT CAPTURE3  Pin RPB11
    PPSInput(4, IC2, RPB10); //INPUT CAPTURE2  Pin RPB10
    PPSLock;
    OpenTimer23(T23_ON | T23_IDLE_CON | T2_PS_1_1 | T23_32BIT_MODE_ON | T23_SOURCE_INT, 481928);
    OpenOC1(OC_ON | OC_IDLE_CON | OC_TIMER_MODE32 | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
    OpenOC2(OC_ON | OC_IDLE_CON | OC_TIMER_MODE32 | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
    OpenCapture2(IC_ON | IC_IDLE_CON | IC_FEDGE_RISE | IC_CAP_32BIT | IC_SP_EVERY_EDGE);
    SetDCOC1PWM(0);
    SetDCOC2PWM(0);

    int myValue = 0;

    int counter = 0;

    while (1) {
        if (counter == 0) {
            ReadCapture2(&myValue);
            SetDCOC1PWM(myValue);
            counter++;
        } else {
            counter = 0;
        }
    }
}
